import serial
import time  # 必须导入时间模块

everyone_command = bytes([0x00,0xFF,0X66,0X6B])
def motor_wei_zhi(ser, action, speed, circle_count):
    """
    :param ser: 已打开的串口对象
    :param action: 动作字符 (F/L/B/R/T/Y)
    :param speed: 速度值 (0-65535)
    :param circle_count: 旋转圈数 (浮点数)
    :param everyone_command: 同步指令 (默认4个0x00)
    """
    # 参数校验
    if action not in ['F', 'L', 'B', 'R', 'T', 'Y']:
        raise ValueError("Invalid action, must be F/L/B/R/T/Y")
    if not 0 <= speed <= 0xFFFF:
        raise ValueError("Speed out of range (0-65535)")

    # 计算脉冲数
    pulse_count = int(circle_count * 3200)
    
    # 方向映射表
    direction_map = {
        'F': [0x00, 0x01, 0x01, 0x00],
        'L': [0x01, 0x01, 0x00, 0x00],
        'B': [0x01, 0x00, 0x00, 0x01],
        'R': [0x00, 0x00, 0x01, 0x01],
        'T': [0x00, 0x00, 0x00, 0x00],  # 停止
        'Y': [0x01, 0x01, 0x01, 0x01]   # 特殊模式
    }
    
    directions = direction_map[action]
    motor_ids = [0x01, 0x02, 0x03, 0x04]

    # 构造并发送每条电机指令
    for motor_id, direction in zip(motor_ids, directions):
        # 拆分速度值
        speed_high = (speed >> 8) & 0xFF
        speed_low = speed & 0xFF
                
        # 构造指令帧
        command = bytes([
            motor_id,        # 电机ID
            0xFD,            # 指令头
            direction,       # 方向
            speed_high,     # 速度高字节
            speed_low,      # 速度低字节
            0x0A,            # 加速度
            (pulse_count >> 24) & 0xFF,   # 脉冲数
            (pulse_count >> 16) & 0xFF,
            (pulse_count >> 8) & 0xFF,
            pulse_count & 0xFF,
            0x00,            # 预留位
            0x03,            # 校验位
            0x6B             # 结束符
        ])
        
        # 发送指令
        time.sleep(0.02)
        ser.write(command)
        time.sleep(0.02)
        ser.flush()  # 确保数据发送完毕

    # 发送同步指令
    time.sleep(0.02)
    ser.write(everyone_command)
    time.sleep(0.02)
    ser.flush()

# 打开串口
ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.5)

# 检查串口是否已经打开
if ser.isOpen():
    print("串口打开成功")

# 写入十六进制数据（注意要使用字节格式）
data_to_run = bytes([0x01, 0xF6, 0x14, 0xFF, 0x00, 0x6B])
data_to_en = bytes([0x01, 0xF3, 0xAB,0x01,0X00,0x6B])
data_to_dis = bytes([0x01, 0xF3, 0xAB,0x00,0X00,0x6B])
time.sleep(0.02)
def main():
    # 初始化串口
    try:
        ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.5)
        if ser.isOpen():
            print("串口打开成功")
    except Exception as e:
        print(f"串口打开失败: {str(e)}")
        return

    # 发送使能指令


    print("""控制指令：
    F - 前进
    B - 后退
    L - 左移
    R - 右移
    Q - 退出""")

    while True:
        try:
            cmd = input("请输入指令：").upper()
            
            if cmd == 'Q':
                print("正在退出...")
                break
                
            if cmd not in ['F', 'B', 'L', 'R']:
                print("无效指令，请重新输入")
                continue
            time.sleep(0.02)
            # 执行运动指令
            motor_wei_zhi(
                ser=ser,
                action=cmd,
                speed=20,
                circle_count=1
            )
            time.sleep(0.02)
            print(f"已执行 {cmd} 方向运动")
            
        except KeyboardInterrupt:
            break

    # 发送失能指令并关闭
    ser.close()
    print("串口已关闭")

if __name__ == "__main__":
    main()